/*
**Copyright (C) 2022, HITCRT_VISION, all rights reserved.
*/
#include "EKF.h"
namespace hitcrt {
/**
 * @brief 初始化滤波器
 * 滤波器的测量误差矩阵和模型误差矩阵将被设为对角阵,对角线上值全一样
 * @param processCov:模型误差矩阵对角线值;measureCov:测量误差矩阵对角线值
 * @return
 * @author WWW
 * -phone:17390600977;qq:1922039181
 */
void EKF::init(const std::vector<boost::any> &param) {
    try {
        if (param.size() != 2) {
            std::string errorMessage = "init():";
            throw paramNumberError(errorMessage);
        }
    } catch (const std::exception &ex) {
        std::cerr << ex.what() << std::endl;
        return;
    }
    const std::vector<double> processCov = boost::any_cast<std::vector<double>>(param[0]);
    const std::vector<double> measureCov = boost::any_cast<std::vector<double>>(param[1]);
    if (processCov.size() == 1) {
        setIdentity(m_processNoiseCov, cv::Scalar::all(processCov[0]));
    } else if (processCov.size() == m_stateDimension) {
        setIdentity(m_processNoiseCov, cv::Scalar::all(1));
        for (int i = 0; i < m_stateDimension; i++) {
            m_processNoiseCov.at<double>(i, i) = processCov[i];
        }
    } else {
        return;
    }
    if (measureCov.size() == 1) {
        setIdentity(m_measurementNoiseCov, cv::Scalar::all(measureCov[0]));
    } else if (measureCov.size() == m_measureDimension) {
        setIdentity(m_measurementNoiseCov, cv::Scalar::all(1));

        for (int i = 0; i < m_measureDimension; i++) {
            m_measurementNoiseCov.at<double>(i, i) = measureCov[i];
        }
    } else {
        return;
    }

    m_measurement = cv::Mat::zeros(m_measureDimension, 1, CV_64F);
    setIdentity(m_measurementMatrix);
    setIdentity(m_errorCovPost, cv::Scalar::all(1));

    if (measureCov.size() == 1) {
        setIdentity(m_errorCovPost, cv::Scalar::all(measureCov[0]));
    } else if (measureCov.size() == m_measureDimension) {
        setIdentity(m_errorCovPost, cv::Scalar::all(1));
        double m_deltaTime = 5 / 1000;
        for (int i = 0; i < m_measureDimension; i++) {
            m_errorCovPost.at<double>(i, i) = measureCov[i] / (pow(m_deltaTime, i));
        }
    } else {
        return;
    }
}
/**
 * @brief 设置滤波器状态转移矩阵
 * @param m_stateDimension:模型维度;4为匀速，6为匀加速
 * @return
 * @author www
 * -phone:17390600977;qq:1922039181
 */
void EKF::setTransition() {
    // clang-format off
    const cv::Mat matrix =(cv::Mat_<double>(6, 6) << 
        1, 0, m_deltaTime,           0, 0.5 * m_deltaTime * m_deltaTime,                               0, 
        0, 1,           0, m_deltaTime,                               0, 0.5 * m_deltaTime * m_deltaTime,
        0, 0,           1,           0,                     m_deltaTime,                               0, 
        0, 0,           0,           1,                  0, m_deltaTime, 
        0, 0,           0,           0,                               1,                               0,
        0, 0,           0,           0,                               0,                               1);

    // clang-format on
    m_transitionMatrix = matrix(cv::Rect(0, 0, m_stateDimension, m_stateDimension));
}
/**
 * @brief 设置滤波器观测矩阵(雅克比矩阵)
 * @param m_stateDimension:模型维度;4为匀速，6为匀加速
 * @param m_measureDimension：观测维度;2无速度，3含角速度,4含径向速度
 * @return {null}
 * @author www
 * -phone:17390600977;qq:1922039181
 */
void EKF::setMeasureMatrix() {
    //中间变量
    double px = m_statePrior.at<double>(0);
    double py = m_statePrior.at<double>(1);
    double vx = m_statePrior.at<double>(2);
    double vy = m_statePrior.at<double>(3);
    double c1 = px * px + py * py;
    double c2 = sqrt(c1);
    double c3 = c1 * c2;

    cv::Mat matrix = cv::Mat(m_measureDimension, m_stateDimension, CV_64F);
    setIdentity(matrix);
    // clang-format off
    if (abs(c1) > MINI){
        matrix = (cv::Mat_<double>(4, 6) << 
                                -(py / c1),                                 (px / c1),      0,     0, 0, 0, 
                                 (px / c2),                                 (py / c2),      0,     0, 0, 0, 
(py*py*vx - px*px*vy + 2*px*py*vx)/(c1*c1),(py*py*vx - px*px*vx - 2*px*py*vy)/(c1*c1), -py/c1, px/c1, 0, 0,
             py * (vx * py - vy * px) / c3,             px * (vy * px - vx * py) / c3,  px/c2, py/c2, 0, 0);
        // clang-format on
    } else { //防止除以0
        std::cout << "Don't device by zero!!" << std::endl;
    }
    m_measurementMatrix = matrix(cv::Rect(0, 0, m_measureDimension, m_stateDimension));
}
/**
 * @brief 状态向量坐标转换(非线性方程)
 * @param m_stateDimension:模型维度;4为匀速，6为匀加速
 * @param m_measureDimension：观测维度;2速度，3含角速度，4含径向速度
 * @return {null}
 * @author www
 * -phone:17390600977;qq:1922039181
 */
void EKF::setTransform(const cv::Mat &state) {
    //中间变量
    double px = state.at<double>(0);
    double py = state.at<double>(1);
    double vx = state.at<double>(2);
    double vy = state.at<double>(3);
    double c1 = px * px + py * py;
    double h1 = atan2(py, px);
    double h2 = sqrt(c1);
    double h3, h4;
    if (fabs(h2) < MINI) { //防止除以0
        std::cout << "Don't device by zero!!" << std::endl;
    } else {
        h3 = (px * vy - py * vx) / c1;
        h4 = (px * vx + py * vy) / h2;
    }
    if (m_measureDimension == 2) {
        m_measurement = (cv::Mat_<double>(2, 1) << h1, h2);
    } else if (m_measureDimension == 3) {
        m_measurement = (cv::Mat_<double>(3, 1) << h1, h2, h3);
    } else if (m_measureDimension == 4) {
        m_measurement = (cv::Mat_<double>(4, 1) << h1, h2, h3, h4);
    } else {
        std::cout << "m_measureDimension is error!!" << std::endl;
        return;
    }
}
/**
 * @brief EKF完整滤波过程
 * @param {null}
 * @return {null}
 * @author www
 * -phone:17390600977;qq:1922039181
 */
void EKF::predict() {
    stateUpdate();
    setMeasureMatrix();
    setTransform(m_statePrior);
    measureUpdate();
}
/**
 * @brief 设置滤波器参数
 * @param dt:预测时间;measureCov:测量误差矩阵对角线值；measure测量数据
 * @return
 * @author www
 * -phone:17390600977;qq:1922039181
 */
void EKF::setParam(const std::vector<boost::any> &param) {
    try {
        if (param.size() != 3) {
            std::string errorMessage = "setParam():";
            throw paramNumberError(errorMessage);
        }
    } catch (const std::exception &ex) {
        std::cerr << ex.what() << std::endl;
        return;
    }
    m_deltaTime = boost::any_cast<double>(param[0]);
    const std::vector<double> measureCov = boost::any_cast<std::vector<double>>(param[1]);
    const std::vector<double> measure = boost::any_cast<std::vector<double>>(param[2]);
    if (measureCov.size() == 1) {
        setIdentity(m_measurementNoiseCov, cv::Scalar::all(measureCov[0]));
    } else if (measureCov.size() == m_measureDimension) {
        setIdentity(m_measurementNoiseCov, cv::Scalar::all(1));

        for (int i = 0; i < m_measureDimension; i++) {
            m_measurementNoiseCov.at<double>(i, i) = measureCov[i];
        }
    } else {
        return;
    }
    if (measure.size() == 1) {
        setIdentity(m_measurement, cv::Scalar::all(measure[0]));
    } else {
        setIdentity(m_measurement, cv::Scalar::all(1));
        for (int i = 0; i < m_measureDimension; i++) {
            m_measurement.at<double>(i) = measure[i];
        }
    }
    setTransition();
}
/**
 * @brief 获取滤波结果
 * 注意：结果并非后验状态估计，需要做一步坐标转换
 * @param {null}
 * @return {cv::Mat&} 观测值的滤波结果
 * @author www
 * -phone:17390600977;qq:1922039181
 */
const cv::Mat &EKF::getResult() {
    setTransform(m_statePost);
    return m_transformation;
}
} // namespace hitcrt